#include <cv.h>
#include <highgui.h>
#include <ctype.h>
#include <stdio.h>
#include <stdlib.h>
#include <luke.h>
#include <Generic.h>
#include <ePuck.h>
#include <Control.h>
#include <Vision.h>

#include <iostream>
#include <fstream>
#include <vector>
#include <stdarg.h>
#include <math.h>
#include <list> 
#include <pthread.h> 
#include <time.h>
#include <ctime>
#include <unistd.h>
#include <sys/time.h>
 
 
// Enable/disable debug prints
//#define DEBUG
void 
PrintMessage( bool Print, const char* Format, ... )
{
  if( Print )
  {
    va_list args;
    va_start(args, Format);
    vprintf( Format, args );
    va_end(args);
  }
}

void
DisplayImage( cv::Mat& Frame )
{
  printf(" Display image called, something should appear in DisplayStream\n" ); 
  cv::imshow("DisplayStream", Frame );
  // Sleep to display frame properly 
  cv::waitKey( 5 );
}

int 
main( int argc, char** argv )
{
  bool Debug      = true;
  int  Iteration  = 0;
  SResult Result;
  
  timeval Start;
  timeval End;
  timeval Diff;
 
  if( 2 != argc )
  {
    printf( "Usage: %s files/image.ext\n", argv[ 0 ] );
    return -1;
  }

  // Connect to the ePuck
  ePuck::Device ePuck;

  // Create controller
  CControl Controller;

  // Create vision object
  CVision Vision;

  if( Result )
  {
    Result.SetResult
    (
      ePuck::EResultOk == ePuck.Connect( "/dev/rfcomm0" )
    ) << "Error connecting to ePuck, will continue anyway" << std::endl;
    PrintMessage( !Result, Result.GetMessage().c_str() ); 
    Result.SetResult( true );
  }

  if( Result )
  {
    Controller.SetAlpha( 640, 480 );
    Result = Vision.Initialize( 640, 480, argv[ 1 ] );
    PrintMessage( !Result, Result.GetMessage().c_str() ); 
  }

	// init display
  // TODO: Move to init or remain here?? questions, questions, questions....
	cv::namedWindow( "DisplayStream", 1 );
	cv::namedWindow( "DisplayMatch",  1 );
	
  // init frames
	cv::Mat Frame, GrayFrame/*, MatchFrame*/;

  
	// Show screen containing match image
  if( Result )
  {
	  cv::Mat DisplayFrameMatch( Vision.GetMatchFrame() );
    Vision.DrawKeyframes( DisplayFrameMatch, CVision::EKeypointsOriginal );
	  cv::imshow("DisplayMatch", DisplayFrameMatch );
  }


	int Errors = 0;
  std::list<cv::Mat> History;
	std::ofstream File("ControllerLog.txt", std::ios::out | std::ios::trunc);

	std::ofstream Benchmark("Benchmark.csv", std::ios::out | std::ios::trunc);
  Benchmark << "Extract,Homography,Match,Control,Smooth,Loop,FPS" << std::endl;

  std::vector<double> FPS;
  std::vector<unsigned long> TControl;
  std::vector<unsigned long> TSmooth; 
  std::vector<unsigned long> TLoop; 

  // Number of skipped frames
  unsigned SkippedFrames = 0;

	while( Result )
	{
    // Time used for local measurements
    timeval LocalStart;
    timeval LocalEnd;
    timeval LocalDiff;

    // Time of day
    gettimeofday(&Start, 0);

    // Current Iteration
    Iteration++;
		
    // Homography
    cv::Mat Homography; 

    if( Result )
    {
      CVision::EStatus Status = CVision::EStatusOk;
      Result = Vision.Update( Status, Homography, File );
     
      if( CVision::EStatusErrorNonFatal == Status )
      {
        PrintMessage( true, "Non fatal error: " );
        PrintMessage( !Result, Result.GetMessage().c_str() ); 
        Result.SetResult( true );
        SkippedFrames = 0;
        DisplayImage( Vision.GetCameraFrame( ) );
        continue;
      }
      else if( CVision::EStatusObjectLost == Status )
      {
        PrintMessage( true, ".\n" );
        Result.SetResult( true );
        SkippedFrames++;
        
        //ePuck.Move( Left, Right );
        if( SkippedFrames >= 10 )
        {
          SkippedFrames = 0;
          // searching moet afhankelijk worden van de rotatievariable in control.cpp
          int Left = 0, Right = 0;
          int Rotation=Controller.GetAvoidRotation();
          if(Rotation==-1){
            Left = 40, Right = -40;
            PrintMessage( true, "Searching right after left avoidance \n" );
          }
          else if(Rotation==1){
            Left = -40, Right = 40;
            PrintMessage( true, "Searching left after right avoidance \n" );

          }
          else{
            Left = -40, Right = 40;
            PrintMessage( true, "Searching without correction \n" );
          }
          ePuck.Move( Left, Right );
        }
        DisplayImage( Vision.GetCameraFrame( ) );
        continue;
      }
    }

    CVision::EObstructionStatus Status;
    double DistanceX = 0.0, DistanceY = 0.0;
    if( Result ) 
    {
      // Create web-cam frame
      cv::Mat DisplayFrame( Vision.GetCameraFrame() );

      // Check if the blue ball is in the path of the robot
      Vision.DetectObstruction( Status, DistanceX, DistanceY );

      // Draw key-frames onto the web-cam frame
      Vision.DrawKeyframes( DisplayFrame, CVision::EKeypointsMatched );
      
      // show image in display				
      DisplayImage( DisplayFrame );
    }
    
    if( Result )
    {
      if( CVision::EObstructionStatusObjectDetected == Status )
      {
        printf( " Object detected: %.2f cm ahead, %.2f cm to the right\n", DistanceX, DistanceY );
      }
    }

    // Generate control signal
    gettimeofday(&LocalStart, 0);
		int Left = 0, Right = 0;
    if( Result )
    {
      bool NonFatal = true;
      Result = Controller.GenerateControlSignal( Homography, Left, Right, File, NonFatal, DistanceX, DistanceY );
      if( !Result && NonFatal )
      {
        Result.SetResult( true );
        continue;
      }
    }
	  gettimeofday(&LocalEnd, 0);
    Generic::SubstractTime( &Diff, &LocalEnd, &LocalStart );
    TControl.push_back( Diff.tv_usec );

    // Drive the monster-truck
	  gettimeofday(&LocalStart, 0);
    if( Result )
    {
      Controller.SmoothControlSignal( Left, Right, 10 );
      Result.SetResult
      (
        ePuck::EResultOk == ePuck.Move( Left, Right )
      ) << " Failed to move monster truck, continue anyway" << std::endl;
      
      if( !Result )
      {
        Result.SetResult( true );
      }
    }
	  gettimeofday(&LocalEnd, 0);
    Generic::SubstractTime( &Diff, &LocalEnd, &LocalStart );
    TSmooth.push_back( Diff.tv_usec );
    
	  gettimeofday(&End, 0);
    Generic::SubstractTime( &Diff, &End, &Start );
		TLoop.push_back( Diff.tv_usec );
    
    // Wait 5ms for a key to be pressed
		if( cv::waitKey( 5 ) >= 0 ) 
		{
			break;
		}
    gettimeofday( &End, 0 );

    if( Diff.tv_sec > 0 )
    {
      printf( "Frame-rate < 1Hz\n" );
      FPS.push_back( 0 );
    }
    else
    {
      printf( "FPS: %f\n", 1000000.0 / Diff.tv_usec );
      FPS.push_back( 1000000.0 / Diff.tv_usec );
    }
	}

  printf( "\n" );

  printf( "Storing benchmark results\n" );
  std::vector<unsigned long>& rTHomography  = Vision.GetBenchmark( 0 );
  std::vector<unsigned long>& rTExtract     = Vision.GetBenchmark( 1 );
  std::vector<unsigned long>& rTMatch       = Vision.GetBenchmark( 2 );
  for( unsigned i = 0; i < rTHomography.size(); i++ )
  {
    //Benchmark << "Extract,Homography,Match" << std::endl;
    Benchmark << rTExtract[ i ] << "," << rTHomography[ i ] << "," << rTMatch[ i ];
    Benchmark << "," << TControl[ i ] << "," << TSmooth[ i ] << "," << TLoop[ i ];
    Benchmark << "," << FPS[ i ] << std::endl;
  }

	if( !Result )
	{
	  printf( "Sorry, we are closing with an error\n: %s", Result.GetMessage().c_str() );
    ePuck.Sound( ePuck::ESoundAuw );
    sleep( 2 );
	}
	
	if( Errors )
	{
		printf( "Errors: %d\n", Errors );
	}
	File.close( );
	Benchmark.close( );
  ePuck.Stop( );
  return Result;
}
